/*
 #
 #  Files       : Matrix.h
 #                ( C++ header file )
 #
 #  Description : The SmallMatrix Library
 #                ( http://code.google.com/p/smallmatrix )
 #
 #  Copyright   : Olivier Juan
 #                ( http://www.mas.ecp.fr/vision/Personnel/juan/ )
 #
 #  License     : CeCILL-C
 #                ( http://www.cecill.info/licences/Licence_CeCILL-C_V1-en.html )
 #
 #  This software is governed by the CeCILL-C license under French law and
 #  abiding by the rules of distribution of free software.  You can  use,
 #  modify and or redistribute the software under the terms of the CeCILL-C
 #  license as circulated by CEA, CNRS and INRIA at the following URL
 #  "http://www.cecill.info".
 #
 #  As a counterpart to the access to the source code and  rights to copy,
 #  modify and redistribute granted by the license, users are provided only
 #  with a limited warranty  and the software's author,  the holder of the
 #  economic rights,  and the successive licensors  have only  limited
 #  liability.
 #
 #  In this respect, the user's attention is drawn to the risks associated
 #  with loading,  using,  modifying and/or developing or reproducing the
 #  software by the user in light of its specific status of free software,
 #  that may mean  that it is complicated to manipulate,  and  that  also
 #  therefore means  that it is reserved for developers  and  experienced
 #  professionals having in-depth computer knowledge. Users are therefore
 #  encouraged to load and test the software's suitability as regards their
 #  requirements in conditions enabling the security of their systems and/or
 #  data to be ensured and,  more generally, to use and operate it in the
 #  same conditions as regards security.
 #
 #  The fact that you are presently reading this means that you have had
 #  knowledge of the CeCILL-C license and that you accept its terms.
 #
*/

#ifndef _SMALLMATRIX_MATRIX_H
#define _SMALLMATRIX_MATRIX_H

#include <SmallMatrix/SmallMatrix.h>

NAMESPACE_BEGIN(SmallMatrix)

template <int M, int N> class Matrix : public Matrix_Base<Matrix<M,N> >
{
private:
	typedef Matrix_Base<Matrix<M,N> > Base;
public:
	template <int IdxR, int IdxC>
	reel& Access() {
		STATIC_ASSERT((IdxR<M)&&(IdxR>=0));
		STATIC_ASSERT((IdxC<N)&&(IdxC>=0));
		return Base::template dataAccess<IdxR*N+IdxC>();
	}
	template <int IdxR, int IdxC>
	const reel& Access() const {
		STATIC_ASSERT((IdxR<M)&&(IdxR>=0));
		STATIC_ASSERT((IdxC<N)&&(IdxC>=0));
		return Base::template dataAccess<IdxR*N+IdxC>();
	}
public:
	INLINE Matrix() : Base(){};
	INLINE Matrix(const reel &v) : Base(v){}
	INLINE Matrix(const reel tab[Base::SIZE]) : Base(tab){}
	INLINE Matrix(const Vector<M>& left,const Vector<N>& right) {
		Matrix<M,N>& ref=*this;
		const Matrix<1,N>& right_M = *reinterpret_cast<const Matrix<1,N>*>(&right);
		matrix_product_Unrolled<M>::template Operation<Matrix<M,N>,Vector<M>,Matrix<1,N> >::compute(ref,left,right_M);
	}
	template <typename TT> INLINE Matrix(const Matrix_Base<TT>& copy) : Base(copy){}
	INLINE Matrix(const Matrix<M,N>& copy) : Base(copy){}
	template <class TT> INLINE Matrix<M,N>& operator=(const Matrix_Base<TT>& copy) {
		Base& ref=*this;
		return ref = copy;
	}
	INLINE Matrix<M,N>& operator=(const Matrix<M,N>& copy) {
		Base& ref=*this;
		return ref = copy;
	}
	INLINE reel& operator()(const int& idxm,const int& idxn);
	INLINE const reel& operator()(const int& idxm,const int& idxn)const;
	//template <typename T1,typename T2> INLINE Matrix<M,N>& prod(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2);
	//template <typename T1,typename T2> INLINE Matrix<M,N>& add(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2);
	//template <typename T1,typename T2> INLINE Matrix<M,N>& subst(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2);
	//template <typename T1> INLINE Matrix<M,N>& add(const Matrix_Base<T1>& M1);
	//template <typename T1> INLINE Matrix<M,N>& subst(const Matrix_Base<T1>& M1);
	//template <typename TT> INLINE Matrix<M,N>& mult(const Matrix_Base<TT>& Two);
	//template <typename TT> INLINE Matrix<M,N>& div(const Matrix_Base<TT>& Two);
	INLINE Matrix<N,M> trans()const;
	Matrix<M,N> Matrix<M,N>::SVD(Vector<N>& W,SquareMatrix<N>& V) const;
};
//! Read/Write Accessor
template <int M,int N> INLINE reel& Matrix<M,N>::operator()(const int& idxm,const int& idxn)
{
	assert((idxm>=0)&&(idxm<M)&&(idxn>=0)&&(idxn<N));
	Matrix<M,N>& ref = *this;
	return ref[idxm*N+idxn];
}
//! Read Only Accessor
template <int M,int N> INLINE const reel& Matrix<M,N>::operator()(const int& idxm,const int& idxn) const
{
	assert((idxm>=0)&&(idxm<M)&&(idxn>=0)&&(idxn<N));
	const Matrix<M,N>& ref = *this;
	return ref[idxm*N+idxn];
}
////! Matrix-Matrix Product in which the result is a Matrix
//template <int M,int N> template <typename T1,typename T2> INLINE Matrix<M,N>& Matrix<M,N>::prod(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2) {
//	Matrix<M,N>& ref = *this;
//	for (int i=0;i<M;i++)
//		for (int j=0;j<N;j++) {
//			ref(i,j)=0;
//			for (int k=0;k<T1::COL;k++)
//				ref(i,j)+=M1(i,k)*M2(k,j);
//		}
//	return ref;
//}
////! Matrix-Matrix Sum in which the result is a Matrix
//template <int M,int N> template <typename T1,typename T2> INLINE Matrix<M,N>& Matrix<M,N>::add(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2) {
//	Matrix<M,N>& ref=*this;
//	for (int i=0;i<M;i++)
//		for (int j=0;j<N;j++)
//			ref(i,j)=M1(i,j)+M2(i,j);
//	return ref;
//}
////! Matrix-Matrix Substract in which the result is a Matrix
//template <int M,int N> template <typename T1,typename T2> INLINE Matrix<M,N>& Matrix<M,N>::subst(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2) {
//	Matrix<M,N>& ref=*this;
//	for (int i=0;i<M;i++)
//		for (int j=0;j<N;j++)
//			ref(i,j)=M1(i,j)-M2(i,j);
//	return ref;
//}
////! Matrix-Matrix Sum in which the result is a Matrix : add M1 to the Matrix
//template <int M,int N> template <typename T1> INLINE Matrix<M,N>& Matrix<M,N>::add(const Matrix_Base<T1>& M1) {
//	Matrix<M,N>& ref=*this;
//	for (int i=0;i<M;i++)
//		for (int j=0;j<N;j++)
//			ref(i,j)+=M1(i,j);
//	return ref;
//}
////! Matrix-Matrix Substract in which the result is a Matrix : substract M1 to the Matrix
//template <int M,int N> template <typename T1> INLINE Matrix<M,N>& Matrix<M,N>::subst(const Matrix_Base<T1>& M1) {
//	Matrix<M,N>& ref=*this;
//	for (int i=0;i<M;i++)
//		for (int j=0;j<N;j++)
//			ref(i,j)-=M1(i,j);
//	return ref;
//}
//template <int M,int N> template <typename TT> INLINE Matrix<M,N>& Matrix<M,N>::mult(const Matrix_Base<TT>& Two) {
//	Matrix<M,N>& ref=*this;
//	for (int i=0;i<M;i++)
//		for (int j=0;j<N;j++)
//			ref(i,j)*=Two(i,j);
//	return ref;
//}
//template <int M,int N> template <typename TT> INLINE Matrix<M,N>& Matrix<M,N>::div(const Matrix_Base<TT>& Two) {
//	Matrix<M,N>& ref=*this;
//	for (int i=0;i<M;i++)
//		for (int j=0;j<N;j++) {
//			assert(Two(i,j)!=0);
//			ref(i,j)/=Two(i,j);
//		}
//	return ref;
//}
//! Matrix Transpose
template <int M,int N> INLINE Matrix<N,M> Matrix<M,N>::trans()const {
	const Matrix<M,N>& ref = *this;
	Matrix<N,M> res;
	matrix_trans_Unrolled<N>::compute(res,ref);
	return res;
}

template <typename T, typename U>
INLINE T SIGN(T a, U b) {
	return b>=0 ? (a >=0 ? a : -a) : (a>=0 ? -a : a);
}

template <typename T>
INLINE T MAX(T a, T b) {
	return a >= b ? a : b;
}
template <typename T>
INLINE T MIN(T a, T b) {
	return a >= b ? b : a;
}

template <int M,int N> Matrix<M,N> Matrix<M,N>::SVD(Vector<N>& W,SquareMatrix<N>& V) const
{
	const Matrix<M,N>& ref=*this;
	Matrix<M,N> U;
	U=ref;
	bool flag;
	int i, its, j, jj, k, l, nm;
	reel anorm, c, f, g, h, s, scale, x, y, z;
	Vector<N> rv1;
	g=scale=anorm=0.0;
	for (i=0; i<N; i++)
	{
		l=i+2;
		rv1[i]=scale*g;
		g=s=scale=0.0;
		if (i<M)
		{
			for (k=i; k<M; k++)
				scale+=fabs(U(k,i));
			if (scale != 0.0)
			{
				for (k=i; k<M; k++)
				{
					U(k,i)=U(k,i)/scale;
					s=s+U(k,i)*U(k,i);
				}
				f=U(i,i);
				g=-SIGN(std::sqrt(s),f);
				h=f*g-s;
				U(i,i)=f-g;
				for(j=l-1; j<N; j++)
				{
					for (s=0.0, k=i; k<M; k++)
						s=s+U(k,i)*U(k,j);
					f=s/h;
					for (k=i; k<M; k++)
						U(k,j)=U(k,j)+(f*U(k,i));
				}
				for (k=i; k<M; k++)
					U(k,i)=U(k,i)*scale;
			}
		}
		W[i]=scale*g;
		g=s=scale=0.0;
		if(i+1<=M && i+1!=N)
		{
			for (k=l-1; k<N; k++)
				scale+=fabs(U(i,k));
			if (scale !=0.0)
			{
				for(k=l-1;k<N;k++)
				{
					U(i,k)=U(i,k)/scale;
					s=s+U(i,k)*U(i,k);
				}
				f=U(i,l-1);
				g=-SIGN(std::sqrt(s),f);
				h=f*g-s;
				U(i,l-1)=f-g;
				for (k=l-1; k<N; k++)
					rv1[k]=U(i,k)/h;
				for(j=l-1; j<M; j++)
				{
					for(s=0.0, k=l-1; k<N; k++)
						s=s+U(j,k)*U(i,k);
					for (k=l-1; k<N; k++)
						U(j,k)=U(j,k)+s*rv1[k];
				}
				for(k=l-1; k<N; k++)
					U(i,k)*= scale;
			}
		}
		anorm=MAX(anorm, (fabs(W[i])+fabs(rv1[i])));
	}
	for (i=N-1; i>=0; i--)
	{
		if(i<N-1)
		{
			if (g!=0.0)
			{
				for(j=l; j<N; j++)
					V(j,i)=(U(i,j)/U(i,l))/g;
				for (j=l; j<N; j++)
				{
					for (s=0.0, k=l; k<N; k++)
						s=s+U(i,k)*V(k,j);
					for (k=l; k<N; k++)
						V(k,j)=V(k,j)+s*V(k,i);
				}
			}
			for (j=l; j<N; j++)
				V(i,j)=V(j,i)=0.0;
		}
		V(i,i)=1.0;
		g=rv1[i];
		l=i;
	}
	for (i=MIN(M,N)-1; i>=0; i--)
	{
		l=i+1;
		g=W[i];
		for(j=l; j<N; j++)
			U(i,j)=0.0;
		if (g!=0.0)
		{
			g=1.0/g;
			for (j=l; j<N; j++)
			{
				for (s=0.0, k=l; k<M; k++)
					s=s+U(k,i)*U(k,j);
				f=(s/U(i,i))*g;
				for (k=i; k<M; k++)
					U(k,j)=U(k,j)+f*U(k,i);
			}
			for(j=i; j<M; j++)
				U(j,i)=U(j,i)*g;
		}
		else for (j=i; j<M; j++)
			U(j,i)=0.0;
		++U(i,i);
	}
	for (k=N-1; k>=0; k--)
	{
		for (its =0;its<30;its++)
		{
			flag=true;
			for (l=k; l>=0; l--)
			{
				nm=l-1;
				if (fabs(rv1[l])+anorm==anorm)
				{
					flag=false;
					break;
				}
				if (fabs(W[nm])+anorm ==anorm) break;
			}
			if (flag)
			{
				c=0.0;
				s=1.0;
				for(i=l;i<k+1; i++)
				{
					f=s*rv1[i];
					rv1[i]=c*rv1[i];
					if (fabs(f)+anorm == anorm) break;
					g=W[i];
					h=pythag(f,g);
					W[i]=h;
					h=1.0/h;
					c=g*h;
					s=-f*h;
					for (j=0; j<M; j++)
					{
						y=U(j, nm);
						z=U(j,i);
						U(j,nm)=y*c+z*s;
						U(j,i)=z*c-y*s;
					}
				}
			}
			z=W[k];
			if (l==k)
			{
				if(z<0.0)
				{
					W[k]=-z;
					for (j=0; j<N; j++)
						V(j,k)=-1*V(j,k);
				}
				break;
			}
			if (its==29)
			{
				cout<<"pas de convergence";
				break;
			}
			x=W[l];
			nm=k-1;
			y=W[nm];
			g=rv1[nm];
			h=rv1[k];
			f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
			g=pythag(f, 1.0);
			f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
			c=s=1.0;
			for(j=l; j<=nm; j++)
			{
				i=j+1;
				g=rv1[i];
				y=W[i];
				h=s*g;
				g=c*g;
				z=pythag(f,h);
				rv1[j]=z;
				c=f/z;
				s=h/z;
				f=x*c+g*s;
				g=g*c-x*s;
				h=y*s;
				y=y*c;
				for (jj=0; jj<N; jj++)
				{
					x=V(jj,j);
					z=V(jj,i);
					V(jj,j)=x*c+z*s;
					V(jj,i)=z*c-x*s;
				}
				z=pythag(f,h);
				W[j]=z;
				if (z)
				{
					z=1.0/z;
					c=f*z;
					s=h*z;
				}
				f=c*g+s*y;
				x=c*y-s*g;
				for (jj=0; jj<M; jj++)
				{
					y=U(jj,j);
					z=U(jj,i);
					U(jj,j)=y*c+z*s;
					U(jj,i)=z*c-y*s;
				}
			}
			rv1[l]=0.0;
			rv1[k]=f;
			W[k]=x;
		}
	}
	return U;
}

NAMESPACE_END

#endif
